perm filename TRAJ.SAI[SYS,HE]6 blob
sn#032856 filedate 1973-03-30 generic text, type T, neo UTF8
00100 DEFINE WRITE="DEB_HAND";
00200 REQUIRE "VECTOR.SAI[SYS,HE]" SOURCE_FILE;
00300 EXTERNAL INTEGER _SKIP_;
00400 DEFINE AVT (A, S, CA, SA)="1.0, -CA, SA, A, 1.0, CA, -SA, A, 0.0, SA, CA, S, [3] 0.0, 1.0, 0.0 ";
00500 DEFINE AVP (A, S, CA, SA)="-CA, SA, A, CA, -SA, A";
00600 DEFINE AVS (A, CA, SA, CT, ST)="CT, -CA*ST, SA*ST, A*CT, ST, CA*CT, -SA*CT, A*ST,
00700 0.0, SA, CA, 1.0, [3] 0.0, 1.0, 0.0 ";
00800 DEFINE QT = "0.0, -1.0, 0.0, 0.0, 1.0, [12] 0.0 ";
00900 DEFINE QS = "[11] 0.0, 1.0, [5] 0.0 ";
01000 DEFINE JDEF (M, X, Y, Z, IXX, IYY, IZZ) =".5*(-IXX+IYY+IZZ), 0.0, 0.0, M*X, 0.0,
01100 .5*(IXX-IYY+IZZ), 0.0, M*Y, 0.0, 0.0, .5*(IXX+IYY-IZZ), M*Z, M*X, M*Y, M*Z, M, 0.0 ";
01200 REQUIRE "YELLOW.SAI[SYS,HE]" SOURCE_FILE;
01300 SAFE REAL ARRAY U1,T[0:101];
01400 SAFE REAL ARRAY DIAG[1:7];
01500 SAFE INTEGER ARRAY COEFF[0:'1037];
01600 SAFE REAL ARRAY ARRIVE_ARM,DEPART_ARM[1:4];
01700 REAL F1_ARM,F2_ARM,OBJECT_MASS,OBJECT_KXX,OBJECT_KYY,OBJECT_KZZ;
01800 INTEGER T1_ARM,T2_ARM,MERGE;
01900 DEFINE MAX_SEG="8";
02000 SAFE REAL ARRAY POSITION[1:6,0:MAX_SEG];
02100 SAFE REAL ARRAY KOE[1:6,1:(MAX_SEG*3)+6];
02200 SAFE INTEGER ARRAY PERIOD[1:6,1:MAX_SEG];
02300 SAFE INTEGER ARRAY NS[1:6];
02400 SAFE REAL ARRAY POS[0:MAX_SEG];
02500 SAFE REAL ARRAY SOLN[1:(MAX_SEG*3)+6];
02600 SAFE INTEGER ARRAY TIM[1:MAX_SEG];
02700 SAFE STRING ARRAY BANDS[0:'37];
02800 SAFE INTEGER ARRAY TRACK[0:'40];
02900 INTEGER NEXT_BAND,BAND,BAND_NUMBER;
03000 PRELOAD_WITH 0,'1040,0;
03100 SAFE INTEGER ARRAY GOODIE[1:3];
03200 DEFINE UFBWRT="'707000000000";
03300 BOOLEAN FORCED,WAS_FORCED,FAST;
03400 SAFE REAL ARRAY LAST_ARM[1:6];
03500 SAFE REAL ARRAY LAST_TRANS[1:4,1:4];
03600 SAFE REAL ARRAY QA,D[0:16];
03700 SAFE REAL ARRAY TF[1:6];
03800 SAFE REAL ARRAY LU,NR[1:6,1:6];
03900 REAL DIGITS;
04000 INTEGER LOOP,JOINT,DURATION;
04100 SAFE REAL ARRAY REAL_DATA[1:18];
04200 REQUIRE "MATRIX.REL[SYS,HE]" LOAD_MODULE;
04300 REQUIRE "SAILIB.REL[SYS,HE]" LOAD_MODULE;
04400 EXTERNAL SIMPLE PROCEDURE MMOVE(REFERENCE REAL R,A);
04500 EXTERNAL SIMPLE PROCEDURE MTIMES(REFERENCE REAL R,A,B);
04600 EXTERNAL REAL SIMPLE PROCEDURE TRACET(REFERENCE REAL X,Y);
04700 EXTERNAL SIMPLE PROCEDURE DECOMPOSE(INTEGER I; REAL ARRAY NR,LU);
04800 EXTERNAL SIMPLE PROCEDURE SOLVE(INTEGER I;REAL ARRAY NR,LU,DTH);
04900 EXTERNAL SIMPLE PROCEDURE IMPROVE(INTEGER I; REAL ARRAY NR,LU,DA,DTH;REFERENCE REAL DIGITS);
05000 INTEGER SIMPLE PROCEDURE SQAR (INTEGER I);
05100 RETURN ((I-1)*17);
05200
00100 SIMPLE PROCEDURE COMPLETA (INTEGER I;SAFE REAL ARRAY TH);
00200 BEGIN REAL S,C;
00300 INTEGER J,K;
00400 J←SQAR(I);
00500 K←6*(I-1);
00600 S←SIND(TH[I]);
00700 C←COSD(TH[I]);
00800 A[J]←C;
00900 A[J+1]←APAR[K]*S;
01000 A[J+2]←APAR[K+1]*S;
01100 A[J+3]←APAR[K+2]*C;
01200 A[J+4]←S;
01300 A[J+5]←APAR[K+3]*C;
01400 A[J+6]←APAR[K+4]*C;
01500 A[J+7]←APAR[K+5]*S;
01600 END;
01700
01800 SIMPLE PROCEDURE HANDPOS(SAFE REAL ARRAY TH);
01900 BEGIN INTEGER I;
02000 FOR I←1,2,4,5,6 DO COMPLETA(I,TH);
02100 A[SQAR(3)+11]←TH[3];
02200 A[3]←A[3]+SHOLDER[1];
02300 A[7]←A[7]+SHOLDER[2];
02400 MMOVE(A[0],A[0]);
02500 MMOVE(T[0],A[0]);
02600 UNDERFLOW(-1);
02700 FOR I←2 STEP 1 UNTIL 6 DO MTIMES(T[SQAR(I)],T[SQAR(I-1)],A[SQAR(I)]);
02800 UNDERFLOW(0);
02900 END;
03000
03100 SIMPLE PROCEDURE UPDATE_SEG;
03200 BEGIN INTEGER I;
03300 HANDPOS(ARM_VECTOR);
03400 FOR I←3 STEP 1 UNTIL 6 DO
03500 BEGIN
03600 ARRBLT(ARM_LINK[I,1,1],T[SQAR(I)],16);
03700 END;
03800 GRASP←ARM_VECTOR[7];
03900 END;
04000
04100 SIMPLE PROCEDURE NOT_LESS(REFERENCE REAL V;REAL L);
04200 BEGIN
04300 IF V<0.0 ∧ V>-L THEN BEGIN V←0; RETURN END;
04400 IF V≥0.0 ∧ V< L THEN V←0;
04500 END;
04600
00100 SIMPLE PROCEDURE INCREMENT(SAFE REAL ARRAY DTH,DX_DY_DZ;BOOLEAN COMP);
00200 BEGIN PRELOAD_WITH 3,7,11;
00300 SAFE OWN INTEGER ARRAY KEY[1:6];
00400 PRELOAD_WITH [6] 0.0; SAFE OWN REAL ARRAY DIR[1:6];
00500 INTEGER I,J;
00600 REAL MAX,R;
00700 LABEL OUT;
00800 REDUCE(DX_DY_DZ);
00900 IF DX_DY_DZ[1]=0.0 ∧
01000 DX_DY_DZ[2]=0.0 ∧
01100 DX_DY_DZ[3]=0.0 THEN BEGIN FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←0.0;
01200 RETURN;
01300 END;
01400 UNDERFLOW(-1);
01500 ARRBLT(DIR[1],DX_DY_DZ[1],3);
01600 IF ¬COMP THEN BEGIN
01700 IF ABS(T[85+2])<ABS(T[85+6])
01800 THEN BEGIN KEY[4]←2;
01900 KEY[5]←IF ABS(T[85+6])<ABS(T[85+10]) THEN 6 ELSE 10
02000 END
02100 ELSE BEGIN KEY[4]←6;
02200 KEY[5]←IF ABS(T[85+2])<ABS(T[85+10]) THEN 2 ELSE 10
02300 END;
02400 KEY[6]←IF ABS(T[85+4])>ABS(T[85+0])
02500 THEN IF ABS(T[85+8])>ABS(T[85+4]) THEN 9 ELSE 5
02600 ELSE IF ABS(T[85+8])>ABS(T[85+0]) THEN 9 ELSE 1;
02700 MMOVE(U1[SQAR(6)],A[SQAR(6)]);
02800 FOR I←5 STEP -1 UNTIL 2 DO
02900 BEGIN MTIMES(U1[SQAR(I)],A[SQAR(I)],U1[SQAR(I+1)]);
03000 MTIMES(U1[SQAR(I+1)],Q[(IF I=2 THEN 17 ELSE 0)],U1[SQAR(I+1)])
03100 END;
03200 A[3]←A[7]←0.0;
03300 MMOVE(A[0],A[0]);
03400 MTIMES(U1[0],A[0],U1[SQAR(2)]);
03500 MTIMES(U1[SQAR(2)],Q[0],U1[SQAR(2)]);
03600 FOR I←2 STEP 1 UNTIL 6 DO MTIMES(U1[SQAR(I)],T[SQAR(I-1)],U1[SQAR(I)]);
03700 MTIMES(U1[0],Q[0],U1[0]);
03800 FOR I←1 STEP 1 UNTIL 6 DO
03900 FOR J←1 STEP 1 UNTIL 6 DO
04000 NR[J,I]←U1[SQAR(I)+KEY[J]];
04100 DECOMPOSE(6,NR,LU);
04200 END;
04300 SOLVE(6,LU,DIR,DTH);
04400 IMPROVE(6,NR,LU,DIR,DTH,DIGITS);
04500 MAX←0;
04600 FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(DIR[I]))>MAX THEN MAX←R;
04700 FOR I←1 STEP 1 UNTIL 6 DO IF (R←ABS(DTH[I]))>MAX THEN
04800 BEGIN FOR J←I+1 STEP 1 UNTIL 6 DO IF ABS(DTH[I]+DTH[J])/R<0.5 THEN
04900 BEGIN DTH[I]←0;
05000 DTH[J]←0;
05100 GO TO OUT;
05200 END;
05300 END;
05400 OUT: FOR I←1 STEP 1 UNTIL 6 DO BEGIN
05500 DTH[I]←IF I=3 THEN DTH[I] ELSE RAD*DTH[I];
05600 NOT_LESS(DTH[I],@-5);
05700 END;
05800 UNDERFLOW(0);
05900 END;
06000
06100 INTEGER SIMPLE PROCEDURE LIMITS(SAFE REAL ARRAY JOINT);
06200 BEGIN INTEGER I;
06300 FOR I←1 STEP 1 UNTIL 5 DO
06400 IF (STOP[I,1]-JOINT[I])*(JOINT[I]-STOP[I,2])<0.0 THEN RETURN(I);
06500 RETURN (0);
06600 END;
06700
06800 SIMPLE PROCEDURE FAST_WRITE;
06900 IF FAST
07000 THEN BEGIN LABEL OK;
07100 START_CODE LABEL WRT;
07200 HRRZ 1,GOODIE;
07300 HRRZ 2,COEFF;
07400 MOVEM 2,(1);
07500 MOVE 3,BAND_NUMBER;
07600 MOVEI 4,10;
07700 WRT: UFBWRT 3,(1);
07800 SOJGE 4,WRT;
07900 MOVEI 2,'20;
08000 ADDM 2,2(1);
08100 JUMPGE 4,OK;
08200 END;
08300 USERERR(0,1,"TRAJECTORY WRITE ERROR");
08400 OK: END
08500 ELSE ARRYOUT('16,COEFF[0],'1000);
08600
00100 SIMPLE PROCEDURE FORCE(SAFE REAL ARRAY TQ,FV);
00200 BEGIN
00300 STRING S;
00400 INTEGER I,L;
00500 REAL XS,YS;
00600 SAFE OWN REAL ARRAY F,M,P[1:4];
00700 SAFE OWN REAL ARRAY INV[1:4,1:4];
00800 EXTERNAL SIMPLE PROCEDURE TRANSFORM(REAL ARRAY R;REFERENCE REAL A;REAL ARRAY V);
00900 EXTERNAL SIMPLE PROCEDURE INVERT(REFERENCE REAL R,A);
01000 SIMPLE PROCEDURE COLVECT(REAL ARRAY R;INTEGER L,I);
01100 BEGIN INTEGER K;
01200 FOR K←1 STEP 1 UNTIL 3 DO R[K]←A[L+(K-1)*4+(I-1)];
01300 R[4]←1.0;
01400 END;
01500
01600 PUSH_FORMAT(9,1);
01700 ARRBLT(F[1],FV[1],3);
01800 ARRBLT(M[1],FV[4],3);
01900 INVERT(INV[1,1],T[SQAR(6)]);
02000 F[4]←M[4]←0.0;
02100 TRANSFORM(F,INV[1,1],F);
02200 TRANSFORM(M,INV[1,1],M);
02300 A[3]←A[7]←0.0;
02400 FOR L←6 STEP -1 UNTIL 1 DO
02500 BEGIN
02600 F[4]←M[4]←0.0;
02700 TRANSFORM (F,A[SQAR(L)],F);
02800 COLVECT(P,SQAR(L),4);
02900 F[4]←1.0;
03000 CROSS(P,P,F);
03100 TRANSFORM(M,A[SQAR(L)],M);
03200 M[4]←1.0;
03300 PLUS(M,M,P);
03400 REDUCE(M);
03500 TQ[L]←IF L=3 THEN F[3] ELSE M[3];
03600 END;
03700 A[3]←A[3]+SHOLDER[1];
03800 A[7]←A[7]+SHOLDER[2];
03900 S←"JOINT TORQUES";
04000 FOR I←1 STEP 1 UNTIL 6 DO S←S&(CVF(TQ[I]));
04100 IF TYP_HAND THEN OUTSTR(S&CRLF&CRLF);
04200 POP_FORMAT;
04300 END;
04400
04500
04600 PRELOAD_WITH
04700 '400000200000,
04800 '200000040000,
04900 '100000010000,
05000 '40000002000,
05100 '20000000400,
05200 '10000000100;
05300 SAFE OWN INTEGER ARRAY GO_WORD[1:6];
05400 SIMPLE PROCEDURE BITS(REFERENCE INTEGER DATWD;REAL SIGN;REFERENCE INTEGER DAT);
05500 START_CODE
05600 MOVE 3,-3('17);
05700 MOVE 1,(3);
05800 MOVE 2,@-1('17);
05900 TDO 1,2;
06000 TRZ 1,(2);
06100 LSH 2,1;
06200 SKIPGE -2('17);
06300 TRO 1,(2);
06400 MOVEM 1,(3);
06500 END;
06600
06700 PROCEDURE FREE_JOINT(REFERENCE INTEGER DATWD);
06800 BEGIN INTEGER NF,I,J,K;
06900 REAL MAX;
07000 SAFE OWN INTEGER ARRAY ZF[1:6];
07100 SAFE OWN REAL ARRAY FV,TQ[1:6];
07200 NF←FREE_ARM[0,1];
07300 FOR I←1 STEP 1 UNTIL NF DO
07400 BEGIN
07500 ARRBLT(FV[1],FREE_ARM[I,1],6);
07600 FORCE(TQ,FV);
07700 MAX←0.0;
07800 FOR J←1 STEP 1 UNTIL 6 DO
07900 BEGIN LABEL L1;
08000 FOR K←1 STEP 1 UNTIL I-1 DO IF ZF[K]=J THEN GO TO L1;
08100 IF ABS(TQ[J]/F0[J])>MAX THEN
08200 BEGIN
08300 MAX←ABS(TQ[J]/F0[J]);
08400 ZF[I]←J;
08500 END;
08600 L1: END;
08700 BITS(DATWD,TQ[ZF[I]],GO_WORD[ZF[I]]);
08800 END;
08900 END;
09000
09100 SIMPLE INTEGER PROCEDURE RUNTIME(SAFE REAL ARRAY DTH);
09200 BEGIN
09300 INTEGER T,TIME,I;
09400 TIME←0;
09500 FOR I←1 STEP 1 UNTIL 6 DO BEGIN
09600 T←ABS(DTH[I])*TIMFAC[I];
09700 IF T>TIME THEN TIME←T END;
09800 RETURN(TIME+20);
09900 END "RUNTIME";
10000
00100 SIMPLE PROCEDURE SCHEINMAN (SAFE REAL ARRAY RES,TH);
00200 BEGIN
00300 LABEL F2;
00400 REAL M,IXX,IYY,IZZ;
00500 SAFE OWN REAL ARRAY JI,TQ[1:6];
00600 EXTERNAL REAL SIMPLE PROCEDURE INNER(REFERENCE REAL A,B);
00700 REAL Z; INTEGER I,DIFF,J,K,L,TJ;
00800 RES[1]←0;ARRBLT(RES[2],RES[1],6);
00900 WAS_FORCED←FORCED;
01000 FORCED←FALSE;
01100 FORCED←OBJECT_MASS;
01200 M←9.33*OBJECT_MASS+M6;
01300 Z←(M6*Z6)/M;
01400 IXX←I6XX+9.33*OBJECT_MASS*OBJECT_KXX↑2;
01500 IYY←I6YY+9.33*OBJECT_MASS*OBJECT_KYY↑2;
01600 IZZ←I6ZZ+9.33*OBJECT_MASS*OBJECT_KZZ↑2;
01700 I←SQAR(6);
01800 JMAT[I]←0.5*(-IXX+IYY+IZZ);
01900 JMAT[I+5]←0.5*(IXX-IYY+IZZ);
02000 JMAT[I+10]←0.5*(IXX+IYY-IZZ);
02100 JMAT[I+11]←JMAT[I+14]←M*Z;
02200 JMAT[I+15]←M;
02300 COMPLETA(1,TH);
02400 COMPLETA(2,TH);
02500 A[SQAR(3)+11]←TH[3];
02600 COMPLETA(4,TH);
02700 COMPLETA(5,TH);
02800 COMPLETA(6,TH);
02900 UNDERFLOW(-1);
03000 MTIMES(U1[0],Q[0],A[0]);
03100 MTIMES(D[0],U1[0],JMAT[0]);
03200 JI[1]←TRACET(D[0],U1[0]);
03300 MMOVE(T[0],A[0]);
03400 for J ← 2 step 1 until 6 do begin
03500 TJ←SQAR(J);
03600 DIFF←IF J=3 THEN 17 ELSE 0;
03700 MTIMES (QA[0], Q[DIFF], A[TJ]);
03800 MTIMES (U1[TJ], T[SQAR(J-1)], QA[0]);
03900 for I← 1 step 1 until J-1 do MTIMES (U1[SQAR(I)],U1[SQAR(I)], A[TJ]);
04000 MTIMES(T[TJ],T[SQAR(J-1)],A[TJ]);
04100 F2: FOR I←1 STEP 1 UNTIL J DO RES[I]←RES[I]
04200 +0.107*INNER(U1[SQAR(I)+8],JMAT[TJ+12]);
04300 For K← 1 step 1 until J do begin
04400 MTIMES (D[0],U1[SQAR(K)],JMAT[TJ]);
04500 JI[K]←JI[K] + TRACET(D[0],U1[SQAR(K)]);
04600 END;
04700 END;
04800 T[88]←T[88]+SX;T[92]←T[92]+SY;
04900 FOR I←1 STEP 1 UNTIL 6 DO IF FORCE_ARM[I] THEN BEGIN
05000 FORCE(TQ,FORCE_ARM);
05100 FORCED←TRUE;
05200 FOR I←1 STEP 1 UNTIL 6 DO RES[I]←RES[I]+TQ[I];
05300 DONE;
05400 END;
05500 FOR I←1 STEP 1 UNTIL 6 DO NOT_LESS(RES[I],1.0@-5);
05600 FOR I←1 STEP 1 UNTIL 6 DO RES[I]←(RES[I] LAND '777777000000) LOR (SQRT(JI[I]) LSH -18);
05700 I←'770000252502;
05800 IF NNUL THEN I←I+'1000000;
05900 IF ¬(FORCED ∨ WAS_FORCED) THEN I←I+'2000000;
06000 FREE_JOINT(I);
06100 ARRBLT(RES[7],I,1);
06200 UNDERFLOW(0);
06300 END;
00100 SIMPLE PROCEDURE FLUSH(BOOLEAN FINISH;SAFE REAL ARRAY TS);
00200 BEGIN INTEGER I;
00300 IF ¬FRST_OPEN ∨ FINISH
00400 THEN BEGIN
00500 IFC WAVE THENC
00600 FOR I←1 STEP 1 UNTIL FREEL
00700 DO FOR J←1 STEP 1 UNTIL PTR3
00800 DO IF EQU(REF[J],LABELS[I])
00900 THEN BEGIN
01000 START_CODE
01100 MOVE 1,STACK;
01200 ADD 1,J;
01300 HRRE 1,-1(1);
01400 MOVEM 1,N END;
01500 N←PTRS[I]-J+N;
01600 REF[J]←NULL;
01700 IF N+J<1 ∨ N+J>PTR3+1
01800 THEN BEGIN
01900 OUTSTR(LABEL_LINE[I]&LABELS[I]&" OUT OF RANGE"&'15&'12);
02000 N←PTR3+1-J END;
02100 STACK[J]←(N LAND '777777) LOR (STACK[J] LAND '777000000);
02200 LABEL_LINE[I]←NULL END;
02300 FOR I←1 STEP 1 UNTIL PTR3
02400 DO IF LENGTH(REF[I])
02500 THEN BEGIN OUTSTR(CODE_LINE[I]&REF[I]&" UNDEFINED"&'15&'12);
02600 STACK[I]←(PTR3+1-I) LOR '102000000;
02700 REF[I]←NULL;
02800 LABEL_LINE[I]←NULL END;
02900 FREEL←0;
03000 FOR I←1 STEP 1 UNTIL PTR3 DO
03100 IF (J←(STACK[I] LAND '777000000))='102000000 ∨ J='16000000 THEN BEGIN
03200 START_CODE
03300 MOVE 1,STACK;
03400 ADD 1,I;
03500 HRRE 1,-1(1);
03600 MOVEM 1,N END;
03700 IF ¬N ∨ N+I<1 ∨ N+I>PTR3+1 THEN BEGIN
03800 OUTSTR(CODE_LINE[I]&"JUMP OUT OF RANGE"&CRLF);
03900 STACK[I]←(PTR3+1-I) LOR '102000000 END;
04000 END;
04100 FOR I←1 STEP 1 UNTIL MAC DO BBEG[I]←LLAB[I]←1;
04200 ENDC
04300 IF PTR3+PTR4≥PTR2 THEN USERERR(0,1,"TRAJECTORY FILE TOO LONG");
04400 ARRBLT(COEFF[PTR4+1],STACK[1],PTR3);
04500 COEFF[PTR4+PTR3+1]←0;
04600 I←-(PTR4+1);
04700 ARRBLT(COEFF[0],I,1);
04800 COEFF['1000]←0;
04900 PTR4←PTR3←0;
05000 FAST_WRITE;
05100 END ELSE FRST_OPEN←FALSE;
05200 IF FINISH THEN BEGIN
05300 IF ¬FAST THEN RELEASE('16);
05400 RETURN END;
05500 ARRTRAN(TH,TS);
05600 SCHEINMAN(DIAG,TH);
05700 END;
05800
05900 SIMPLE PROCEDURE FLUSHP(INTEGER N;SAFE REAL ARRAY TH);
06000 BEGIN FRST_OPEN←FALSE;
06100 IF PTR3+PTR4+N≥511 THEN FLUSH(0,TH);
06200 END;
06300
00100 SIMPLE PROCEDURE REVOLVE(SAFE REAL ARRAY P,O; REAL TH);
00200 BEGIN
00300 SAFE OWN REAL ARRAY OP,A,T[1:4];
00400 UNIT(O,O);
00500 SCALE(OP,O,DOT(P,O));
00600 DIFFERENCE(A,P,OP);
00700 CROSS(T,O,A);
00800 SCALE(T,T,SIND(TH));
00900 SCALE(P,A,COSD(TH));
01000 PLUS(P,P,T);
01100 PLUS(P,P,OP);
01200 REDUCE(P);
01300 END;
01400 PROCEDURE RESET_CONO;
01500 BEGIN
01600 FREE_ARM[0,1]←FORCE_ARM[1]←OBJECT_MASS←OBJECT_KXX←OBJECT_KYY←OBJECT_KZZ←0;
01700 NNUL←0;
01800 ARRBLT(FORCE_ARM[2],FORCE_ARM[1],5);
01900 END;
02000
02100
02200 SIMPLE PROCEDURE LIFTOFF(SAFE REAL ARRAY TH,DTH,DIR;REFERENCE INTEGER N,T1);
02300 BEGIN INTEGER I,J;
02400 REAL F;
02500 LABEL L2;
02600 SAFE OWN REAL ARRAY TT[1:6];
02700 F←1.0;
02800 IF LIMITS(TH) THEN BEGIN
02900 FOR I←1 STEP 1 UNTIL 5 DO DTH[I]←
03000 IF TH[I]≥STOP[I,2] THEN STOP[I,2]-TH[I]
03100 ELSE IF TH[I]≤STOP[I,1] THEN STOP[I,1]-TH[I]
03200 ELSE 0;
03300 GO TO L2;
03400 END;
03500 HANDPOS(TH);
03600 INCREMENT(DTH,DIR,FALSE);
03700 FOR I←1 STEP 1 UNTIL 6 DO TT[I]←TH[I]+DTH[I];
03800 I←LIMITS(TT);
03900 IF I THEN
04000 BEGIN FOR J←1,2 DO
04100 IF (TT[I]-STOP[I,J])*(STOP[I,J]-TH[I])≥0
04200 THEN
04300 BEGIN F←0.9*(STOP[I,J]-TH[I])/(TT[I]-TH[I]);
04400 DTH[I]←DTH[I]*F;
04500 DONE;
04600 END;
04700 END;
04800 L2: N←RUNTIME(DTH);IF T1>N THEN N←T1;
04900 END;
05000
00100 EXTERNAL SIMPLE PROCEDURE PACK(REFERENCE INTEGER PTR;INTEGER PERIOD;REFERENCE REAL BUF);
00200 SIMPLE PROCEDURE PACK_UP;BEGIN
00300 INTEGER P1,I,J,K;
00400 REAL R1,R2;
00500 SAFE OWN INTEGER ARRAY LOOPP,NXT,ALT[1:6];
00600 SAFE OWN REAL ARRAY BUF[0:4];
00650 COEFF[PTR4←PTR4+1]←DURATION;
00700 STACK[PTR3←PTR3+1]←'21000000 LOR PTR4;
00800 IF MERGE THEN BEGIN
00900 STACK[PTR3]←STACK[PTR3] LOR '400000000;
01000 STACK[PTR3]↔STACK[PTR3-1];
01100 MERGE←FALSE END;
01200 BUF[1]←0;BUF[2]←0;
01300 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
01400 BUF[0]←POSITION[JOINT,0];
01500 BUF[3]←KOE[JOINT,1];BUF[4]←KOE[JOINT,2];
01600 PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,1],BUF[0]);
01700 NXT[JOINT]←PTR4 END;
01800 COEFF[PTR4←PTR4+1]←PTR2;
01900 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
02000 BUF[4]←0;
02100 K←IF LOOP THEN 7 ELSE NS[JOINT]-1;
02200 FOR I←2 STEP 1 UNTIL K DO BEGIN
02300 BUF[0]←POSITION[JOINT,I-1];
02400 FOR J←1 STEP 1 UNTIL 3 DO BUF[J]←KOE[JOINT,(3*I)-4+J];
02500 PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,I],BUF[0]);
02600 COEFF[NXT[JOINT]]←(PTR4 LSH 18) LOR COEFF[NXT[JOINT]];
02700 NXT[JOINT]←PTR4;
02800 IF LOOP THEN BEGIN
02900 IF I=3 THEN LOOPP[JOINT]←PTR4;
03000 IF I=6 THEN ALT[JOINT]←PTR4 END;
03100 IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←PTR2←PTR2-7 END;
03200 BUF[0]←POSITION[JOINT,I-1];
03300 BUF[1]←-4.0*(R1←KOE[JOINT,(3*I)-2])+3.0*(R2←KOE[JOINT,(3*I)-3]);
03400 BUF[2]← 6.0*R1-3.0*R2;
03500 BUF[3]←-4.0*R1 +R2;
03600 BUF[4]← R1;
03700 PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,I],BUF[0]);
03800 COEFF[NXT[JOINT]]←(PTR4 LSH 18) LOR COEFF[NXT[JOINT]];
03900 BUF[4]←0;
04000 IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←PTR2←PTR2-7;
04100 IF LOOP THEN BEGIN
04200 BUF[0]←POSITION[JOINT,6];
04300 FOR J←1 STEP 1 UNTIL 4 DO BUF[J]←KOE[JOINT,3*I-2+J];
04400 PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,3],BUF[0]);
04500 NXT[JOINT]←PTR4;
04600 COEFF[P1←ALT[JOINT]]←(PTR4 LSH 27) LOR COEFF[P1];
04700 IF JOINT=6 THEN COEFF[P1+1]←'100000000000 LOR (LOOP LSH 18) LOR COEFF[P1+1];
04800 IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←499;
04900 BUF[0]←POSITION[JOINT,0];
05000 FOR J←1 STEP 1 UNTIL 4 DO BUF[J]←KOE[JOINT,3*I+2+J];
05100 PACK(COEFF[PTR4←PTR4+4],PERIOD[JOINT,3],BUF[0]);
05200 COEFF[NXT[JOINT]]←(PTR4 LSH 18) LOR COEFF[NXT[JOINT]];
05300 COEFF[PTR4]←(LOOPP[JOINT] LSH 18) LOR COEFF[PTR4];
05400 IF JOINT=6 THEN COEFF[PTR4←PTR4+1]←473 END END;
05500 END;
05600
05700 INTEGER SIMPLE PROCEDURE QUADROOT(REAL A0,A1,A2,A3;REFERENCE REAL ANG,T);BEGIN
05800 REAL DISC;
05900 INTEGER I;
06000 IF A3 ∧ (DISC←(A2/(3*A3))↑2-A1/(3*A3))≥0 THEN DISC←SQRT(DISC) ELSE RETURN(0);
06100 T←DISC-A2/(3*A3);
06200 FOR I←0,1 DO BEGIN
06300 IF 0< T <1.0 THEN
06400 BEGIN
06500 ANG←(((A3*T)+A2)*T+A1)*T+A0;
06600 IF ANG>STOP[JOINT,2] THEN BEGIN ANG←STOP[JOINT,2];RETURN(1) END;
06700 IF ANG<STOP[JOINT,1] THEN BEGIN ANG←STOP[JOINT,1];RETURN(1) END;
06800 END;
06900 T←-(T+2*A2/3*A3);
07000 END;
07100 RETURN(0);
07200 END;
07300
00100 IFC WAVE THENC
00200 SIMPLE PROCEDURE STEP_ARM(INTEGER JOINT;REAL DTH;INTEGER TIME);
00300 BEGIN
00400 INTEGER I;
00500 IF ARM_EXECUTE THEN RETURN;
00600 FLUSH(0,LAST_ARM);
00700 FOR I←1 STEP 1 UNTIL 6 DO
00800 BEGIN
00900 POSITION[I,0]←LAST_ARM[I];
01000 NS[I]←2;
01100 PERIOD[I,1]←10;
01200 PERIOD[I,2]←IF TIME>60 THEN TIME-10 ELSE 50;
01300 IF I=JOINT THEN POSITION[I,1]←POSITION[I,2]←LAST_ARM[I]←LAST_ARM[I]+DTH
01400 ELSE POSITION[I,1]←POSITION[I,2]←LAST_ARM[I];
01500 END;
01600 COEFF[511]←GO_WORD[JOINT]+2;
01700 ARRBLT(COEFF[PTR2←PTR2-7],COEFF[PTR2+7],7);
01800 ARRBLT(COEFF[PTR2←PTR2-7],COEFF[PTR2+7],7);
01900 KOE[1,1]←0;
02000 ARRBLT(KOE[1,2],KOE[1,1],6*((MAX_SEG*3)+6)-1);
02100 PACK_UP;
02200 DURATION←10;
02300 END;
02400
02500 ENDC
02600
02700 SIMPLE PROCEDURE POLY(INTEGER N);BEGIN
02800 SAFE OWN REAL ARRAY A,LU[1:(3*MAX_SEG)+6,1:(3*MAX_SEG)+6];
02900 SAFE OWN REAL ARRAY B[1:(3*MAX_SEG)+6];
03000 INTEGER I,J,K,P1,N3;
03100 REAL T,T2;
03200 SIMPLE PROCEDURE THREE(INTEGER I,J;REAL T);
03300 BEGIN
03400 FOR K←J STEP 1 UNTIL J+2 DO A[I+2,K]←1.0;
03500 A[I,J]←-T;
03600 A[I+1,J+1]←-(A[I+4,J+1]←2.0*(T2←T*T));
03700 A[I+4,J+2]←6.0*T2;
03800 A[I+3,J+2]←3.0*T;
03900 A[I+3,J+1]←2.0*T;
04000 A[I+3,J]←T;
04100 END;
04200
04300 SIMPLE PROCEDURE FOUR(INTEGER I,J;REAL T);
04400 BEGIN
04500 FOR K←J STEP 1 UNTIL J+3 DO A[I+2,K]←1.0;
04600 A[I,J]←-T;
04700 A[I+1,J+1]←-(A[I+4,J+1]←2.0*(T2←T*T));
04800 A[I+4,J+2]←6.0*T2;
04900 A[I+4,J+3]←12.0*T2;
05000 A[I+3,J+3]←4.0*T;
05100 A[I+3,J+2]←3.0*T;
05200 A[I+3,J+1]←2.0*T;
05300 A[I+3,J]←T;
05400 END;
05500
05600 UNDERFLOW(-1);
05700 N3←N*3;
05800 T←1.0/TIM[1];
05900 T2←T*T;
06000 A[1,1]←A[1,2]←1.0;
06100 A[2,1]←3.0*T;
06200 A[2,2]←4.0*T;
06300 A[3,1]←6.0*T2;
06400 A[3,2]←12.0*T2;
06500 FOR J←1 STEP 1 UNTIL N-2 DO
06600 THREE((3*J)-1,(3*J),1.0/TIM[J+1]);
06700 A[N3-4,N3-3]←-3.0*(T←1.0/TIM[N]);
06800 A[N3-4,N3-2]←4.0*T;
06900 A[N3-3,N3-3]←6.0*(T2←T*T);
07000 A[N3-3,N3-2]←-12.0*T2;
07100 A[N3-2,N3-3]←1.0;
07200 A[N3-2,N3-2]←-1.0;
07300 FOR J←1 STEP 1 UNTIL N DO
07400 B[(J*3)-2]←POS[J]-POS[J-1];
07500 P1←N3-2;
07600 IF LOOP THEN BEGIN
07700 FOUR(N3-1,N3-1,(T←1/TIM[N-2]));
07800 FOUR(N3+2,N3+3,T);
07900 FOR J←6,7,8 DO BEGIN
08000 A[N3+5,J]←A[5,J];
08100 A[N3+6,J]←A[6,J] END;
08200 FOR J←N3-9 STEP 1 UNTIL N3-7 DO BEGIN
08300 A[N3-1,J]←A[N3-7,J];
08400 A[N3,J]←A[N3-6,J] END;
08500 B[N3+1]←B[N3-2]+B[N3-5];
08600 B[N3+4]←B[1]+B[4];
08700 P1←P1+8;
08800 END;
08900 DECOMPOSE(P1,A,LU);
09000 SOLVE(P1,LU,B,SOLN);
09100 IMPROVE(P1,A,LU,B,SOLN,T2);
09200 UNDERFLOW(0);
09300 END;
09400 PRELOAD_WITH 0.5, 0.5, 0.25, 1.0, 1.0, 2.5;
09500 SAFE REAL ARRAY OSHOOT[1:6];
09600
09700 BOOLEAN SIMPLE PROCEDURE OVERSHOOT(REAL A3,A4,DELTA,OFF;REFERENCE REAL T,D);
09800 RETURN(A4 ∧ 0+OFF< (T←-(3*A3)/(4*A4)) <1+OFF
09900 ∧ ABS(D←(IF(D←-A3*T↑3/4)*DELTA<0 THEN DELTA+D ELSE D))>OSHOOT[JOINT]);
10000
00100 INTEGER SIMPLE PROCEDURE KISEKI;BEGIN
00200 INTEGER P2S,N,I,J,OVER;
00300 REAL ANG,T;
00400 SIMPLE PROCEDURE BUMP_UP(INTEGER I;REAL ANG,T);
00500 BEGIN INTEGER J;
00600 OVER←OVER+1;
00800 FOR J←MAX_SEG STEP -1 UNTIL I+1 DO BEGIN
00900 POSITION[JOINT,J]←POSITION[JOINT,J-1];
01000 PERIOD[JOINT,J]←PERIOD[JOINT,J-1] END;
01100 POSITION[JOINT,I]←ANG;
01200 PERIOD[JOINT,I]←T*PERIOD[JOINT,I];
01300 IF PERIOD[JOINT,I]<1 THEN PERIOD[JOINT,I]←1;
01400 PERIOD[JOINT,I+1]←PERIOD[JOINT,I+1]-PERIOD[JOINT,I] END;
01450 P2S←PTR2-7;
01500 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
01600 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
01700 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
01800 POLY(N←NS[JOINT]);
01900 OVER←0;
02000 IF OVERSHOOT(SOLN[1],SOLN[2],POS[1]-POS[0],0,T,ANG)
02100 THEN BUMP_UP(2,POS[1]+ANG,(1.0-T)*TIM[1]/TIM[2]);
02200 IF OVERSHOOT(SOLN[J←(N-1)*3],SOLN[J+1],POS[N-1]-POS[N],-1.0,T,ANG)
02300 THEN BUMP_UP(N+OVER-1,POS[N-1]+ANG,1.0-(1.0+T)*TIM[N]/TIM[N-1]);
02400 IF OVER THEN BEGIN
02500 NS[JOINT]←N+OVER;
02600 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
02700 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
02800 POLY(NS[JOINT])END;
02900 ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+4) END;
03000 FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,1];
03100 SCHEINMAN(DIAG,TH);
03200 ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
03300 DIAG[7]←DIAG[7] LOR '2000000;
03400 IF NS[6]=5 ∨ (NS[6]=4 ∧ PERIOD[6,2]≤PERIOD[6,3]) THEN ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
03500 FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,NS[J]-1];
03600 SCHEINMAN(DIAG,TH);
03700 DIAG[7]←DIAG[7] LOR '2000000;
03800 ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
03900 IF NS[6]=5 ∨ (NS[6]=4 ∧ PERIOD[6,2]>PERIOD[6,3]) THEN ARRBLT(COEFF[PTR2←PTR2-7],COEFF[PTR2+7],7);
04000 FOR J←1 STEP 1 UNTIL 6 DO TH[J]←POSITION[J,NS[J]];
04100 SCHEINMAN(DIAG,TH);
04200 DIAG[7]←DIAG[7] LOR '2000000;
04300 ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
04400 FOR JOINT←2 STEP 1 UNTIL 5 DO BEGIN
04500 OVER←0;
04600 FOR I←NS[JOINT]-1 STEP -1 UNTIL 2 DO
04700 IF QUADROOT(POSITION[JOINT,I-1],KOE[JOINT,(I*3)-3],
04800 KOE[JOINT,(I*3)-2],KOE[JOINT,(I*3)-1],ANG,T)
04900 THEN BUMP_UP(I,ANG,T);
05000 NS[JOINT]←NS[JOINT]+OVER;
05100 IF OVER THEN BEGIN
05200 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
05300 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
05400 POLY(NS[JOINT]);
05500 ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+4) END END;
05600 PTR2←P2S;
05700 PACK_UP;
05800 RETURN(0) END;
05900
00100 SIMPLE MESSAGE PROCEDURE TRAJECTORY(REAL ARRAY T0,TF);
00200 BEGIN
00300 INTEGER MT,I,J,N;
00400 REAL INT,R;
00500 SAFE OWN REAL ARRAY AI,AF,DTH[1:6];
00600 INTEGER TR1,TR2;
00700 WHILE TF[6]-T0[6]>180.0 DO TF[6]←TF[6]-360.0;
00800 WHILE TF[6]-T0[6]≤-180.0 DO TF[6]←TF[6]+360.0;
00900 FLUSHP(150,T0);
01000 LIFTOFF(T0,AI,DEPART_ARM,TR1,T1_ARM);
01100 LIFTOFF(TF,AF,ARRIVE_ARM,TR2,T1_ARM);
01200 N←IF T2_ARM > 50 THEN T2_ARM ELSE 50;
01300 FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←TF[I]+AF[I]-T0[I]-AI[I];
01400 IF (J←RUNTIME(DTH))>50 THEN N←J;
01500 FOR I←1 STEP 1 UNTIL 6 DO BEGIN
01600 POSITION[I,0]←T0[I];
01700 POSITION[I,1]←T0[I]+AI[I];
01800 POSITION[I,2]←TF[I]+AF[I];
01900 POSITION[I,3]←TF[I];
02000 NS[I]←3;
02100 PERIOD[I,1]←TR1;
02200 PERIOD[I,2]←N;
02300 PERIOD[I,3]←TR1 END;
02400 LOOP←0;
02450 DURATION←N;
02500 IF KISEKI THEN USERERR(0,1,"OVERSHOOT");
02700 ARRTRAN(LAST_ARM,TF);
02800 ARM_TIME←ARM_TIME+N+TR1*2;
02900 FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←DEPART_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
03000 ARRIVE_ARM[4]←DEPART_ARM[4]←1.0;
03100 RESET_CONO;
03200 END;
03300 BOOLEAN SIMPLE PROCEDURE IS_NOT_CLEAR(SAFE REAL ARRAY TRANS,J);
03400 BEGIN SAFE OWN REAL ARRAY IP[1:4];
03500 INTEGER FLAG;
03600 REAL R;
03700 BOOLEAN SIMPLE PROCEDURE NOT_CLEAR(REFERENCE REAL J;SAFE REAL ARRAY V);
03800 BEGIN SAFE OWN REAL ARRAY VT[1:4];
03900 EXTERNAL SIMPLE PROCEDURE TRANSFORM(REAL ARRAY V;REFERENCE REAL T;REAL ARRAY F);
04000 TRANSFORM(VT,J,V);
04100 IF VT[3]<0.375 THEN RETURN(TRUE);
04200 IF (VT[1]-SHOLDER[1])↑2+(VT[2]-SHOLDER[2])↑2<25.0 THEN RETURN(TRUE) ELSE RETURN(FALSE);
04300 END;
04400 IP[1]←IP[3]←0.0;IP[4]←1.0;
04500 FOR R←-2.0,2.0 DO BEGIN IP[2]←R;IF NOT_CLEAR(TRANS[1,1],IP)THEN RETURN(TRUE);END;
04600 J[6]←LAST_ARM[6];
04700 ARM_SOLVE(TRANS,J,FLAG);
04800 IF ¬FLAG THEN RETURN(TRUE);
04900 HANDPOS(J);
05000 IP[2]←0.0;
05100 FOR R←3.0,-3.0 DO BEGIN IP[3]←R;IF NOT_CLEAR(T[SQAR(4)],IP) THEN RETURN(TRUE);END;
05200 IP[3]←-38.0;
05300 IF NOT_CLEAR(T[SQAR(3)],IP) THEN RETURN(TRUE) ELSE RETURN(FALSE);
05400 END;
05500
05600 SIMPLE MESSAGE PROCEDURE MOVE_ARM(REAL ARRAY T;REFERENCE INTEGER FLAG);
05700 BEGIN SAFE OWN REAL ARRAY DTH,J[1:6];
05800 SAFE OWN REAL ARRAY EXF[1:7];
05900 INTEGER K,TIME;
06000 FLAG←0;
06100 IF IS_NOT_CLEAR(T,J) THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;;
06200 FLAG←-1;
06300 IF ARM_EXECUTE THEN BEGIN
06400 FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←J[I]-ARM_VECTOR[I];
06500 WHILE DTH[6]>180.0 DO DTH[6]←DTH[6]-360.0;
06600 WHILE DTH[6]≤-180.0 DO DTH[6]←DTH[6]+360.0;
06700 ARM_MESSAGE[15]←'5000000;
06800 ARRBLT(ARM_MESSAGE[1],DTH[1],6);
06900 SCHEINMAN(EXF,J);
07000 ARRBLT(ARM_MESSAGE[7],EXF[1],7);
07100 TIME←20;
07200 RESET_CONO;
07300 K←RUNTIME(DTH);IF K>TIME THEN TIME←K;
07400 ARM_MESSAGE[14]←TIME;
07500 ARMFN(14);
07600 ARM_EXECUTE←FALSE;
07700 UPDATE_SEG END
07800 ELSE BEGIN
07900 IF TYP_HAND THEN PMAT("TRAJECTORY FROM",LAST_TRANS);
08000 ARRTRAN(LAST_TRANS,T);
08100 IF TYP_HAND THEN PMAT("TRAJECTORY TO",T);
08200 TRAJECTORY(LAST_ARM,J) END;
08300 END;
08400
00100 SIMPLE MESSAGE PROCEDURE GO_ARM(REAL ARRAY T;REFERENCE INTEGER FLAG);
00200 BEGIN SAFE OWN REAL ARRAY TF[1:6];
00300 SAFE OWN REAL ARRAY EXF[1:7];
00400 INTEGER K,TIME,MT,I,J,N,NNULS;
00500 REAL INT,R;
00600 SAFE OWN REAL ARRAY TH,DTH,AI,AF[1:6];
00700 INTEGER TR1,TR2;
00800 FLAG←0;
00900 IF IS_NOT_CLEAR(T,TF) THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;;
01000 FLAG←-1;
01100 IF ARM_EXECUTE THEN BEGIN
01200 MOVE_ARM(T,FLAG);
01300 RETURN END;
01400 ARRTRAN(LAST_TRANS,T);
01500 WHILE TF[6]-LAST_ARM[6]>180.0 DO TF[6]←TF[6]-360.0;
01600 WHILE TF[6]-LAST_ARM[6]≤-180.0 DO TF[6]←TF[6]+360.0;
01700 NNULS←NNUL;
01800 NNUL←-1;
01900 LIFTOFF(LAST_ARM,AI,DEPART_ARM,TR1,T1_ARM);
02000 LIFTOFF(TF,AF,ARRIVE_ARM,TR2,T1_ARM);
02100 FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←TF[I]+AF[I]-LAST_ARM[I]-AI[I];
02200 FOR I←1 STEP 1 UNTIL 6 DO TH[I]←LAST_ARM[I]+AI[I];
02300 FLUSHP(90,LAST_ARM);
02400 IF MAGNITUDE(DEPART_ARM) THEN BEGIN
02500 PTR3←PTR3+1;
02600 IF MERGE THEN BEGIN
02700 STACK[PTR3]←'405000000+(PTR4←PTR4+1);
02800 STACK[PTR3]↔STACK[PTR3-1];
02900 MERGE←FALSE END ELSE
03000 STACK[PTR3]←'5000000+(PTR4←PTR4+1);
03100 ARRBLT(COEFF[PTR4],AI[1],6);
03200 SCHEINMAN(DIAG,TH);
03300 ARRBLT(COEFF[PTR4←PTR4+6],DIAG[1],7);
03400 PTR4←PTR4+7;
03500 COEFF[PTR4]←TR1;
03600 END;
03700 PTR3←PTR3+1;
03800 IF MERGE THEN BEGIN
03900 STACK[PTR3]←'420000000+(PTR4←PTR4+1);
04000 STACK[PTR3]↔STACK[PTR3-1];
04100 MERGE←FALSE END ELSE
04200 STACK[PTR3]←'20000000+(PTR4←PTR4+1);
04300 ARRBLT(COEFF[PTR4],DTH[1],6);
04400 ARRBLT(COEFF[PTR4←PTR4+6],TH[1],6);
04500 FOR I←1 STEP 1 UNTIL 6 DO TH[I]←TF[I]+AF[I];
04600 IF ¬MAGNITUDE(ARRIVE_ARM) THEN NNUL←NNULS;
04650 SCHEINMAN(DIAG,TH);
04700 IF MAGNITUDE(DEPART_ARM) THEN DIAG[7]←DIAG[7] LOR '2000000;
04800 ARRBLT(COEFF[PTR4←PTR4+6],DIAG[1],7);
04900 PTR4←PTR4+7;
05000 COEFF[PTR4]←T2_ARM;
05100 NNUL←NNULS;
05200 IF MAGNITUDE(ARRIVE_ARM) THEN BEGIN
05300 STACK[PTR3←PTR3+1]←'5000000+(PTR4←PTR4+1);
05400 FOR I←1 STEP 1 UNTIL 6 DO AF[I]←-AF[I];
05500 ARRBLT(COEFF[PTR4],AF[1],6);
05600 SCHEINMAN(DIAG,TF);
05700 DIAG[7]←DIAG[7] LOR '2000000;
05800 ARRBLT(COEFF[PTR4←PTR4+6],DIAG[1],7);
05900 PTR4←PTR4+7;
06000 COEFF[PTR4]←TR2;
06100 END;
06200 ARRTRAN(LAST_ARM,TF);
06300 ARM_TIME←ARM_TIME+N+TR1+TR2;
06400 FOR I←1 STEP 1 UNTIL 3 DO ARRIVE_ARM[I]←DEPART_ARM[I]←IF I=3 THEN 3.0 ELSE 0.0;
06500 ARRIVE_ARM[4]←DEPART_ARM[4]←1.0;
06600 T1_ARM←T2_ARM←0;
06700 RESET_CONO;
06800 END;
00100 SIMPLE MESSAGE PROCEDURE DRAW_ARM(REAL ARRAY PROFILE;INTEGER STAT);
00200 BEGIN
00300 SAFE OWN REAL ARRAY TRANS[1:4,1:4];
00400 SAFE OWN REAL ARRAY DTH[1:6];
00500 SAFE OWN REAL ARRAY FAS[0:6,1:6];
00600 SAFE OWN REAL ARRAY ROTS,ROTA,RS,IP,RV,CV,FVV,FV,VT,DIFF[1:4];
00700 REAL AR,RR,THP4,R,MAX;
00800 INTEGER TIME,NF,I,J,K,NT;
00900 EXTERNAL SIMPLE PROCEDURE MOVEV(REAL ARRAY R;REFERENCE REAL A);
01000 IF ARM_EXECUTE THEN BEGIN STAT←4;ARM_EXECUTE←FALSE;RETURN END;
01100 ARRTRAN(TRANS,LAST_TRANS);
01200 CVV(IP,LAST_TRANS,4);
01300 ARRTRAN(FAS,FREE_ARM);
01400 MOVEV(DIFF,PROFILE[1,1]);
01500 MOVEV(RV,PROFILE[2,1]);
01600 IF(AR←IF MAGNITUDE(RV) THEN PROFILE[3,1] ELSE 0) THEN UNIT(RV,RV);
01700 MOVEV(ROTA,PROFILE[5,1]);
01800 IF(RR←IF MAGNITUDE(ROTA) THEN PROFILE[3,2] ELSE 0)
01900 THEN BEGIN UNIT(ROTA,ROTA);
02000 MOVEV(ROTS,PROFILE[4,1]);
02100 DIFFERENCE(IP,IP,ROTS) END;
02200 FVV[1]←FV[1]←FORCE_ARM[1];
02300 FVV[2]←FV[2]←FORCE_ARM[2];
02400 FVV[3]←FV[3]←FORCE_ARM[3];
02500 FVV[4]←FV[4]←1.0;
02600 FLUSH(0,LAST_ARM);
02700 SCHEINMAN(DIAG,LAST_ARM);
02800 ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
02900 FRST_OPEN←TRUE;
03000 R←0;
03100 IF PROFILE[0,3] THEN LOOP←PROFILE[0,3]-1;
03200 IF LOOP THEN NT←8 ELSE BEGIN
03300 NT←4;
03400 R←MAGNITUDE(DIFF);
03500 IF R>15.0 THEN NT←(R/5)+0.5;
03600 NT←IF (MAX←AR/60.0)>NT THEN MAX ELSE NT;
03700 NT←IF (MAX←RR/60.0)>NT THEN MAX ELSE NT END;
03800 IF TYP_HAND THEN OUTSTR(CVS(NT)&" PART TRAJECTORY"&CRLF);
03900 IF NT>8 THEN BEGIN STAT←3;RETURN END;
04000 FOR J←1 STEP 1 UNTIL 6 DO POSITION[J,0]←LAST_ARM[J];
04100 TH[6]←LAST_ARM[6];
04200 FOR I←1 STEP 1 UNTIL NT DO
04300 BEGIN
04400 R←IF I=1
04500 THEN 1/(3*(NT-2))
04600 ELSE (I-1)/(NT-2);
04700 R←IF I=NT-1
04800 THEN R-1/(3*(NT-2))
04900 ELSE IF I=NT
05000 THEN 1.0
05100 ELSE R;
05200 SCALE(VT,DIFF,R);
05300 PLUS(VT,VT,IP);
05400 IF RR THEN BEGIN ROTATE(RS,ROTS,ROTA,R*RR);
05500 PLUS(VT,VT,RS)END;
05600 REDUCE(VT);
05700 CVC(TRANS,4,VT);
05800 IF AR THEN FOR J←1 STEP 1 UNTIL 3 DO BEGIN
05900 CVV(CV,LAST_TRANS,J);
06000 REVOLVE(CV,RV,R*AR);
06100 CVC(TRANS,J,CV) END;
06200 THP4←TH[4];
06300 ARM_SOLVE(TRANS,TH,STAT);
06400 IF ¬STAT THEN BEGIN STAT←1;RETURN END;
06500 IF ABS(THP4-TH[4])>90.0 THEN BEGIN STAT←2;RETURN END;
06600 FOR J←1 STEP 1 UNTIL 6 DO BEGIN
06700 POSITION[J,I]←TH[J];
06800 DTH[I]←POSITION[J,I]-POSITION[J,I-1] END;
06900 IF (K←RUNTIME(DTH))*NT > PROFILE[0,2] THEN PROFILE[0,2]←K*NT;
07000 IF RR THEN BEGIN
07100 FOR K←1 STEP 1 UNTIL FAS[0,1] DO
07200 BEGIN
07300 MOVEV(VT,FAS[K,1]);
07400 VT[4]←1.0;
07500 IF MAGNITUDE(VT)>0.0 THEN
07600 BEGIN
07700 REVOLVE(VT,ROTA,R*RR);
07800 REDUCE(VT);
07900 FOR J←1 STEP 1 UNTIL 3 DO FREE_ARM[K,J]←VT[J] END END;
08000 MOVEV(FV,FVV[1]);
08100 REVOLVE(FV,ROTA,R*RR);
08200 REDUCE(FV);
08300 ARRBLT(FORCE_ARM[1],FV[1],3) END;
08400 SCHEINMAN(DIAG,TH);
08500 ARRBLT(COEFF[PTR2←PTR2-7],DIAG[1],7);
08600 END;
08700 TIME←(PROFILE[0,2]/NT)+0.5;
08800 FOR JOINT←1 STEP 1 UNTIL 6 DO BEGIN
08900 NS[JOINT]←NT;
09000 FOR J←1 STEP 1 UNTIL NT DO PERIOD[JOINT,J]←TIME;
09100 ARRBLT(POS[0],POSITION[JOINT,0],NS[JOINT]+1);
09200 ARRBLT(TIM[1],PERIOD[JOINT,1],NS[JOINT]);
09300 POLY(NS[JOINT]);
09400 ARRBLT(KOE[JOINT,1],SOLN[1],(NS[JOINT]*3)+6) END;
09500 PTR2←486;
09600 PACK_UP;
09700 DURATION←TIME;
09800 ARRTRAN(LAST_ARM,TH);
09900 ARRTRAN(LAST_TRANS,TRANS);
10000 ARM_TIME←ARM_TIME+TIME*((NT-2)*LOOP+2);
10100 STAT←0;
10200 RESET_CONO;
10300 FRST_OPEN←FALSE;
10400 END;
00100 SIMPLE MESSAGE PROCEDURE START_TRAJECTORY(STRING FILE;INTEGER START);
00200 BEGIN
00300 STRING S;
00400 LABEL FIND,NONE,FOUND;
00500 REAL R;
00600 FRST_OPEN←TRUE;
00700 ARM_TIME←0;
00800 IF ¬START THEN ARRTRAN(LAST_ARM,ARM_VECTOR);
00900 WHILE LAST_ARM[6]>180.0 DO LAST_ARM[6]←LAST_ARM[6]-360.0;
01000 WHILE LAST_ARM[6]≤180.0 DO LAST_ARM[6]←LAST_ARM[6]+360.0;
01100 HANDPOS(LAST_ARM);
01200 ARRBLT(LAST_TRANS[1,1],T[SQAR(6)],16);
01300 IF FAST THEN BEGIN
01400 FIND: IF NEXT_BAND>'37 THEN GO TO NONE;
01500 FOR BAND←0 STEP 1 UNTIL NEXT_BAND-1 DO IF EQU(FILE,BANDS[BAND])THEN GO TO FOUND;
01600 TRACK[BAND]←CALL('100000000000 LOR BAND,"UFBGET");
01700 IF ¬_SKIP_ THEN GO TO NONE;
01800 NEXT_BAND←BAND+1;
01900 BANDS[BAND]←FILE;
02000 FOUND: GOODIE[3]←0;
02100 BAND_NUMBER←TRACK[BAND];
02200 END ELSE NONE:BEGIN
02300 FAST←FALSE;
02400 OPEN('16,"DSK",'17,0,0,120,BREAK,EOF);
02500 ENTER('16,FILE&".TRJ",BREAK);
02600 END;
02700 COEFF[0]←0; ARRBLT(COEFF[1],COEFF[0],'1037);
02800 PTR4←PTR3←0;
02900 PTR2←512;
03000 END;
03100
03200 SIMPLE MESSAGE PROCEDURE CLOSE_TRAJECTORY;
03300 FLUSH(1,TH);
03400
03500 SIMPLE MESSAGE PROCEDURE ARM_POSITION;
03600 BEGIN
03700 ARMPOS;
03800 UPDATE_SEG;
03900 END;
04000
00100 SIMPLE MESSAGE PROCEDURE OPEN_HAND(REAL OPENING);
00200 IF ARM_EXECUTE
00300 THEN BEGIN ARM_MESSAGE[1]←'1000000 LOR (OPENING LSH -18);
00400 HANDFN;
00500 ARM_EXECUTE←FALSE;
00600 UPDATE_SEG END
00700 ELSE BEGIN
00800 FLUSHP(100,LAST_ARM);
00900 STACK[PTR3←PTR3+1]←'1000000 LOR (OPENING LSH -18) END;
01000
01100 SIMPLE MESSAGE PROCEDURE WOBBLE_HAND(REAL WOBBLEING);
01200 IF ARM_EXECUTE
01300 THEN BEGIN ARM_MESSAGE[1]←'14000000 LOR (WOBBLEING LSH -18);
01400 HANDFN;
01500 ARM_EXECUTE←FALSE;
01600 UPDATE_SEG END
01700 ELSE BEGIN FLUSHP(1,LAST_ARM);STACK[PTR3←PTR3+1]←'14000000 LOR (WOBBLEING LSH -18) END;
01800
01900 SIMPLE MESSAGE PROCEDURE PLACE_ARM;
02000 BEGIN SAFE OWN REAL ARRAY DIR[1:4],DTH[1:6];
02100 INTEGER I;
02200 FOR I←1 STEP 1 UNTIL 3 DO DIR[I]←IF I=3 THEN -0.03 ELSE 0;
02300 DIR[4]←1.0;
02400 INCREMENT(DTH,DIR,FALSE);
02500 IF ARM_EXECUTE
02600 THEN BEGIN ARM_MESSAGE[7]←'4000000;
02700 ARRBLT(ARM_MESSAGE[1],DTH[1],6);
02800 ARMFN(6);
02900 ARM_EXECUTE←FALSE;
03000 UPDATE_SEG END
03100 ELSE BEGIN FLUSHP(7,LAST_ARM);
03200 STACK[PTR3←PTR3+1]←'4000000+(PTR4←PTR4+1);
03300 ARRBLT(COEFF[PTR4],DTH[1],6);
03400 PTR4←PTR4+5 END;
03500 END;
03600
03700 SIMPLE MESSAGE PROCEDURE SEARCH_ARM(REAL R);
03800 BEGIN SAFE OWN REAL ARRAY DIR[1:4],DTH[1:6];
03900 INTEGER I;
04000 FOR I←1 STEP 1 UNTIL 3 DO DIR[I]←IF I=1 THEN R ELSE 0;
04100 DIR[4]←1.0;
04200 INCREMENT(DTH,DIR,FALSE);
04300 IF ARM_EXECUTE THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
04400 FLUSHP(40,LAST_ARM);
04500 STACK[PTR3←PTR3+1]←'15000000+(PTR4←PTR4+1);
04600 ARRBLT(COEFF[PTR4],DTH[1],6);
04700 PTR4←PTR4+6;
04800 DIR[1]↔DIR[2];
04900 INCREMENT(DTH,DIR,TRUE);
05000 ARRBLT(COEFF[PTR4],DTH[1],6);
05100 PTR4←PTR4+5;
05200 END;
05300
05400 SIMPLE MESSAGE PROCEDURE PARK_ARM;
05500 BEGIN SAFE OWN REAL ARRAY DTH[1:6],EXF[1:7],TT[1:4,1:4];
05600 INTEGER K,TIME;
05700 IF ¬ARM_EXECUTE THEN
05800 BEGIN
06000 HANDPOS(V0);
06100 ARRBLT(TT[1,1],T[SQAR(6)],16);
06200 ARRIVE_ARM[3]←0.0;
06400 GO_ARM(TT,K);
06500 END ELSE BEGIN
06600 ARM_EXECUTE←FALSE;
06700 FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←V0[I]-ARM_VECTOR[I];
06800 WHILE DTH[6]>180.0 DO DTH[6]←DTH[6]-360.0;
06900 WHILE DTH[6]≤-180.0 DO DTH[6]←DTH[6]+360.0;
07000 ARM_MESSAGE[15]←'5000000;
07100 ARRBLT(ARM_MESSAGE[1],DTH[1],6);
07200 SCHEINMAN(EXF,V0);
07300 ARRBLT(ARM_MESSAGE[7],EXF[1],7);
07400 TIME←20;
07500 RESET_CONO;
07600 K←RUNTIME(DTH);IF K>TIME THEN TIME←K;
07700 ARM_MESSAGE[14]←TIME;
07800 ARMFN(14);
07900 UPDATE_SEG END
08000 END;
08100
08200 SIMPLE MESSAGE PROCEDURE WAIT_ARM;
08300 IF ¬ARM_EXECUTE THEN BEGIN
08400 FLUSHP(1,LAST_ARM);
08500 STACK[PTR3←PTR3+1]←'3000000 END ELSE ARM_EXECUTE←FALSE;
08600
08700 SIMPLE MESSAGE PROCEDURE CLOSE_HAND(REAL DIST);
08800 IF ARM_EXECUTE
08900 THEN BEGIN ARM_MESSAGE[1]←'2000000 LOR (DIST LSH -18);
09000 HANDFN;
09100 ARM_EXECUTE←FALSE;
09200 UPDATE_SEG END
09300 ELSE BEGIN FLUSHP(100,LAST_ARM);STACK[PTR3←PTR3+1]←'2000000 LOR (DIST LSH -18)END;
09400
00100 SIMPLE MESSAGE PROCEDURE CHANGE_ARM(REAL ARRAY DIR;REAL DIST;
00200 REAL ARRAY AXIS;REAL DEG;INTEGER TIME;REFERENCE INTEGER FLAG);
00300 BEGIN INTEGER J,N;
00400 SAFE OWN REAL ARRAY AV,TQ,DTH[1:6];
00500 SAFE OWN REAL ARRAY EXF[1:7];
00600 SAFE OWN REAL ARRAY TT[1:4,1:4];
00700 SAFE OWN REAL ARRAY VT[1:4];
00800 IF ARM_EXECUTE
00900 THEN BEGIN ARRBLT(TT[1,1],T[85],16);
01000 ARRBLT(AV[1],ARM_VECTOR[1],6) END
01100 ELSE BEGIN ARRTRAN(TT,LAST_TRANS);
01200 ARRTRAN(AV,LAST_ARM)END;
01300 SCALE(VT,DIR,DIST);
01400 REDUCE(VT);
01500 FOR J←1 STEP 1 UNTIL 3 DO TT[J,4]←TT[J,4]+VT[J];
01600 IF DEG ∧ MAGNITUDE(AXIS) THEN BEGIN
01700 UNIT(AXIS,AXIS);
01800 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
01900 CVV(VT,TT,I);
02000 REVOLVE(VT,AXIS,DEG);
02100 CVC(TT,I,VT) END;
02200 END;
02300 DTH[6]←AV[6];
02400 ARM_SOLVE(TT,DTH,FLAG);
02500 IF ¬FLAG THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
02600 FOR I←1 STEP 1 UNTIL 6
02700 DO BEGIN DTH[I]←DTH[I]-AV[I];
02800 AV[I]←DTH[I]+AV[I] END;
02900 IF ARM_EXECUTE
03000 THEN BEGIN ARM_MESSAGE[15]←'5000000;
03100 ARRBLT(ARM_MESSAGE[1],DTH[1],6) END
03200 ELSE BEGIN ARRTRAN(LAST_ARM,AV);
03300 FLUSHP(70,TT);
03400 PTR3←PTR3+1;
03500 IF MERGE THEN BEGIN
03600 STACK[PTR3]←'405000000+(PTR4←PTR4+1);
03700 STACK[PTR3]↔STACK[PTR3-1];
03800 MERGE←FALSE END ELSE
03900 STACK[PTR3]←'5000000+(PTR4←PTR4+1);
04000 ARRBLT(COEFF[PTR4],DTH[1],6) END;
04100 SCHEINMAN(EXF,AV);
04200 IF ARM_EXECUTE
04300 THEN ARRBLT(ARM_MESSAGE[7],EXF[1],7)
04400 ELSE BEGIN ARRBLT(COEFF[PTR4←PTR4+6],EXF[1],7);
04500 PTR4←PTR4+6 END;
04600 RESET_CONO;
04700 J←RUNTIME(DTH);IF J>TIME THEN TIME←J;
04800 IF ARM_EXECUTE
04900 THEN BEGIN ARM_MESSAGE[20]←TIME;
05000 ARMFN(20);
05100 ARM_EXECUTE←FALSE;
05200 UPDATE_SEG END
05300 ELSE BEGIN COEFF[PTR4←PTR4+1]←TIME;
05400 ARM_TIME←ARM_TIME+TIME;
05500 ARRTRAN(LAST_TRANS,TT);
05600 FLAG←IF IS_NOT_CLEAR(LAST_TRANS,DTH)THEN 0 ELSE -1 END;
05700 END;
05800
00100 SIMPLE MESSAGE PROCEDURE CENTER_HAND(REAL DIST);
00200 BEGIN INTEGER J,N;
00300 SAFE OWN REAL ARRAY DIR[1:4];
00400 SAFE OWN REAL ARRAY DTH[1:6];
00500 IF ARM_EXECUTE THEN FOR J←1 STEP 1 UNTIL 3 DO DIR[J]←ARM_LINK[6,J,2]
00600 ELSE FOR J←1 STEP 1 UNTIL 3 DO DIR[J]←LAST_TRANS[J,2]; DIR[4]←1;
00700 INCREMENT(DTH,DIR,FALSE);
00800 FOR I←1 STEP 1 UNTIL 6 DO DTH[I]←DTH[I];
00900 IF ARM_EXECUTE
01000 THEN BEGIN ARM_MESSAGE[8]←'12000000;
01100 ARRBLT(ARM_MESSAGE[1],DIST,1);
01200 ARRBLT(ARM_MESSAGE[2],DTH[1],6);
01300 ARMFN(7);
01400 ARM_EXECUTE←FALSE;
01500 UPDATE_SEG END
01600 ELSE BEGIN FLUSHP(24,LAST_ARM);
01700 STACK[PTR3←PTR3+1]←'12000000+(PTR4←PTR4+1);
01800 ARRBLT(COEFF[PTR4],DIST,1);
01900 ARRBLT(COEFF[PTR4←PTR4+1],DTH[1],6);
02000 PTR4←PTR4+5 END;
02100 END;
02200
02300 SIMPLE MESSAGE PROCEDURE SET_ARM(INTEGER CELL;REAL ARRAY DX_DY_DZ);
02400 BEGIN
02500 REDUCE(DX_DY_DZ);
02600 IF ARM_EXECUTE THEN BEGIN
02700 ARM_MESSAGE[5]←'13000000;
02800 ARM_MESSAGE[1]←CELL;
02900 ARRBLT(ARM_MESSAGE[2],DX_DY_DZ[1],3);
03000 ARMFN(4);
03100 ARM_EXECUTE←FALSE;
03200 UPDATE_SEG END
03300 ELSE BEGIN
03400 FLUSHP(4,LAST_ARM);
03500 STACK[PTR3←PTR3+1]←'13000000+(PTR4←PTR4+1);
03600 COEFF[PTR4]←CELL;
03700 ARRBLT(COEFF[PTR4←PTR4+1],DX_DY_DZ[1],3);
03800 PTR4←PTR4+2 END;
03900 END"SET_ARM";
04000
00100 SIMPLE MESSAGE PROCEDURE DRIVE_ARM(INTEGER JOINT;REAL DEG;INTEGER TIME;REFERENCE INTEGER FLAG);
00200 BEGIN INTEGER I,J,N;
00300 REAL R;
00400 SAFE OWN REAL ARRAY EXF[1:7];
00500 SAFE OWN REAL ARRAY LA,TQ[1:6];
00600 FLAG←FALSE;
00700 IF ARM_EXECUTE THEN ARRBLT(LA[1],ARM_VECTOR[1],6) ELSE ARRTRAN(LA,LAST_ARM);
00800 R←LA[JOINT]+DEG;
00900 IF JOINT<6 ∧ (STOP[JOINT,1]-R)*(R-STOP[JOINT,2])<1 THEN BEGIN ARM_EXECUTE←FALSE;RETURN END;
01000 FLAG←TRUE;
01100 LA[JOINT]←R;
01200 SCHEINMAN(EXF,LA);
01300 IF ARM_EXECUTE
01400 THEN BEGIN ARM_MESSAGE[15]←'5000000;
01500 ARM_MESSAGE[1]←0;ARRBLT(ARM_MESSAGE[2],ARM_MESSAGE[1],5);
01600 ARRBLT(ARM_MESSAGE[JOINT],DEG,1) END
01700 ELSE BEGIN ARRBLT(LAST_TRANS[1,1],T[85],16);
01800 FLUSHP(50,LAST_ARM);
01900 PTR3←PTR3+1;
02000 IF MERGE THEN BEGIN
02100 STACK[PTR3]←'405000000+(PTR4←PTR4+1);
02200 STACK[PTR3]↔STACK[PTR3-1];
02300 MERGE←FALSE END ELSE
02400 STACK[PTR3]←'5000000+(PTR4←PTR4+1);
02500 COEFF[PTR4]←0;
02600 ARRBLT(COEFF[PTR4+1],COEFF[PTR4],5);
02700 ARRBLT(COEFF[PTR4+JOINT-1],DEG,1) END;
02800 N←20+ABS(DEG)*TIMFAC[JOINT];
02900 IF TIME>N THEN N←TIME;
03000 IF ARM_EXECUTE
03100 THEN BEGIN ARRBLT(ARM_MESSAGE[7],EXF[1],7);
03200 ARM_MESSAGE[13]←GO_WORD[JOINT]+2;
03300 IF FREE_ARM[0,1] THEN FREE_JOINT(ARM_MESSAGE[13]);
03400 ARM_MESSAGE[14]←N;
03500 ARMFN(14);
03600 ARM_EXECUTE←FALSE;
03700 UPDATE_SEG END
03800 ELSE BEGIN ARRBLT(COEFF[PTR4←PTR4+6],EXF[1],7);
03900 PTR4←PTR4+6;
04000 COEFF[PTR4]←GO_WORD[JOINT]+2;
04100 IF FREE_ARM[0,1] THEN FREE_JOINT(COEFF[PTR4]);
04200 COEFF[PTR4←PTR4+1]←N;
04300 ARM_TIME←ARM_TIME+N;
04400 ARRTRAN(LAST_ARM,LA) END;
04500 RESET_CONO;
04600 END;
04700
04800 SIMPLE MESSAGE PROCEDURE STOP_ARM(REAL ARRAY F,M;REFERENCE INTEGER STAT);
04900 BEGIN SAFE OWN REAL ARRAY TQ,XF[1:6];
05000 LABEL OK;
05100 INTEGER I;
05200 HANDPOS(LAST_ARM);
05300 REDUCE(F);
05400 ARRBLT(XF[1],F[1],3);
05500 REDUCE(M);
05600 ARRBLT(XF[4],M[1],3);
05700 FORCE(TQ,XF);
05800 FOR I←1 STEP 1 UNTIL 6 DO IF ABS(TQ[I]/F0[I])≥0.2 THEN GO TO OK;
05900 STAT←0;
06000 RETURN;
06100 OK: STAT←-1;
06200 FLUSHP(7,LAST_ARM);
06300 STACK[PTR3←PTR3+1]←'7000000+(PTR4←PTR4+1);
06400 ARRBLT(COEFF[PTR4],TQ[1],6);
06500 PTR4←PTR4+5;
06600 END;
06700
06800 SIMPLE MESSAGE PROCEDURE NO_NULL;NNUL←-1;
06900
00100 SIMPLE MESSAGE PROCEDURE DO_IT(STRING FILE);
00200 BEGIN INTEGER I;
00300 ARM_STATUS←0;
00400 ARM_EXECUTE←FALSE;
00500 FOR I←0 STEP 1 UNTIL NEXT_BAND-1 DO IF EQU(FILE,BANDS[I])THEN DONE;
00600 DOIT(TRACK[I],CVSIX(FILE));
00700 IF ARM_STATUS='50 ∧TRACK[I] THEN BANDS[I]←NULL;
00800 UPDATE_SEG;
00900 END;
01000
01100 SIMPLE MESSAGE PROCEDURE MERGE_ARM;MERGE←TRUE;
01200
01300 SIMPLE MESSAGE PROCEDURE DO_PROCEED(INTEGER SKIP);
01400 BEGIN SAFE OWN REAL ARRAY DTH[1:6];
01500 ARM_EXECUTE←FALSE;
01600 IF ¬ARM_WAIT THEN RETURN;
01700 ARMPROCEED(SKIP);
01800 UPDATE_SEG;
01900 END;
02000
02100 SIMPLE MESSAGE PROCEDURE ARM_CONO(REAL ARRAY ARRIVE,DEPART,OBJECT;INTEGER ARRIVE_TIME,DEPART_TIME);
02200 BEGIN ARRTRAN(ARRIVE_ARM,ARRIVE);
02300 ARRTRAN(DEPART_ARM,DEPART);
02400 OBJECT_MASS←OBJECT[4];
02500 OBJECT_KXX←OBJECT[1];
02600 OBJECT_KYY←OBJECT[2];
02700 OBJECT_KZZ←OBJECT[3];
02800 T2_ARM←DEPART_TIME;
02900 T1_ARM←ARRIVE_TIME;
03000 END;
03100
03200 SIMPLE MESSAGE PROCEDURE SET_TOUCH(BOOLEAN STOP_ON_TOUCH);
03300 IF ¬ARM_EXECUTE THEN BEGIN FLUSHP(1,LAST_ARM);
03400 STACK[PTR3←PTR3+1]←'6000000
03500 END ELSE ARM_EXECUTE←FALSE;
03600
03700 SIMPLE MESSAGE PROCEDURE ARM_SKIPE(INTEGER I);
03800 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'101000000 LOR I ELSE ARM_EXECUTE←FALSE;
03900
04000 SIMPLE MESSAGE PROCEDURE ARM_SKIPN(INTEGER I);
04100 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'103000000 LOR I ELSE ARM_EXECUTE←FALSE;
04200
04300 SIMPLE MESSAGE PROCEDURE ARM_SKIPS(INTEGER I);
04400 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'104000000 LOR I ELSE ARM_EXECUTE←FALSE;
04500
04600 SIMPLE MESSAGE PROCEDURE ARM_JMP(INTEGER I);
04700 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'102000000 LOR ('777777 LAND I) ELSE ARM_EXECUTE←FALSE;
04800
04900 SIMPLE MESSAGE PROCEDURE ARM_AOJ(INTEGER I);
05000 IF ¬ARM_EXECUTE THEN STACK[PTR3←PTR3+1]←'16000000 LOR ('777777 LAND I) ELSE ARM_EXECUTE←FALSE;
05100
00100 SIMPLE MESSAGE PROCEDURE ARM_SAVE(INTEGER LEVEL);
00200 BEGIN
00300 INTEGER I,J,K;
00400 SAFE OWN REAL ARRAY LA[1:6];
00500 IF ARM_EXECUTE THEN ARRBLT(LA[1],ARM_VECTOR[1],6) ELSE ARRTRAN(LA,LAST_ARM);
00600 HANDPOS(LA);
00700 IF ARM_EXECUTE THEN BEGIN ARM_MESSAGE[14]←'10000000;
00800 ARM_MESSAGE[1]←LEVEL END ELSE
00900 BEGIN FLUSHP(20,LAST_ARM);STACK[PTR3←PTR3+1]←'10000000+(PTR4←PTR4+1);
01000 COEFF[PTR4]←LEVEL END;
01100 UNDERFLOW(-1);
01200 MMOVE(U1[SQAR(6)],A[SQAR(6)]);
01300 FOR I←5 STEP -1 UNTIL 2 DO
01400 BEGIN MTIMES(U1[SQAR(I)],A[SQAR(I)],U1[SQAR(I+1)]);
01500 MTIMES(U1[SQAR(I+1)],Q[(IF I=2 THEN 17 ELSE 0)],U1[SQAR(I+1)])
01600 END;
01700 A[3]←A[7]←0.0;
01800 MMOVE(A[0],A[0]);
01900 MTIMES(U1[0],A[0],U1[SQAR(2)]);
02000 MTIMES(U1[SQAR(2)],Q[0],U1[SQAR(2)]);
02100 FOR I←2 STEP 1 UNTIL 6 DO MTIMES(U1[SQAR(I)],T[SQAR(I-1)],U1[SQAR(I)]);
02200 MTIMES(U1[0],Q[0],U1[0]);
02300 K←0;
02400 FOR J←11,7,3 DO
02500 FOR I←6 STEP -1 UNTIL 1 DO
02600 REAL_DATA[K←K+1]←U1[SQAR(I)+J]*(IF I=3 THEN 1.0 ELSE 1.0/RAD);
02700 IF ARM_EXECUTE THEN ARRBLT(ARM_MESSAGE[2],REAL_DATA[1],18)
02800 ELSE ARRBLT(COEFF[PTR4←PTR4+1],REAL_DATA[1],18);
02900 UNDERFLOW(0);
03000 IF ARM_EXECUTE THEN BEGIN ARMFN(19);
03100 ARM_EXECUTE←FALSE;
03200 UPDATE_SEG END ELSE PTR4←PTR4+18;
03300 END;
03400
03500 SIMPLE MESSAGE PROCEDURE ARM_RESTORE(INTEGER LEVEL);
03600 BEGIN
03700 INTEGER I,J,K;
03800 LABEL L1;
03900 SAFE OWN REAL ARRAY DIR[1:4];
04000 SAFE OWN REAL ARRAY LA[1:6];
04100 IF ARM_EXECUTE THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
04200 IF ((I←PTR3)≥1 ∧ STACK[I] LAND '377000000 ='20000000)
04300 ∨((I←PTR3-1)≥1 ∧ STACK[I] LAND '377000000 = '20000000) THEN BEGIN
04400 FOR J←PTR3 STEP -1 UNTIL I DO BEGIN
04500 STACK[J+1]←STACK[J];
04600 END;
04700 PTR3←PTR3+1;
04800 GO TO L1
04900 END;
05000 RETURN;
05100 L1: HANDPOS(LAST_ARM);
05200 STACK[I]←'11000000+(PTR4←PTR4+1);
05300 COEFF[PTR4]←LEVEL;
05400 DIR[4]←1.0;
05500 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
05600 FOR J←1 STEP 1 UNTIL 3 DO DIR[J]←IF J=I THEN 1.0 ELSE 0.0;
05700 INCREMENT(LA,DIR,I>1);
05800 FOR J←1 STEP 1 UNTIL 6 DO
05900 REAL_DATA[I-3+(3*J)]←LA[J] END;
06000 ARRBLT(COEFF[PTR4←PTR4+1],REAL_DATA[1],18);
06100 PTR4←PTR4+18;
06200 END;
06300
06400 SIMPLE MESSAGE PROCEDURE SLAVE_ARM;
06500 BEGIN
06600 INTEGER I,J,K;
06700 SAFE OWN REAL ARRAY DIR[1:4];
06800 SAFE OWN REAL ARRAY LA[1:6];
06900 IF ARM_EXECUTE THEN ARRBLT(LA[1],ARM_VECTOR[1],6) ELSE ARRTRAN(LA,LAST_ARM);
07000 HANDPOS(LA);
07100 IF ARM_EXECUTE THEN ARM_MESSAGE[19]←'17000000 ELSE
07200 BEGIN FLUSHP(25,LAST_ARM);STACK[PTR3←PTR3+1]←'17000000+(PTR4←PTR4+1);
07300 END;
07400 DIR[4]←1.0;
07500 FOR I←1 STEP 1 UNTIL 3 DO BEGIN
07600 FOR J←1 STEP 1 UNTIL 3 DO DIR[J]←IF J=I THEN 1.0 ELSE 0.0;
07700 INCREMENT(LA,DIR,I>1);
07800 FOR J←1 STEP 1 UNTIL 6 DO
07900 REAL_DATA[I-3+(3*J)]←LA[J] END;
08000 IF ARM_EXECUTE THEN BEGIN
08100 ARRBLT(ARM_MESSAGE[1],REAL_DATA[1],18);
08200 ARMFN(18);
08300 ARM_EXECUTE←FALSE;
08400 UPDATE_SEG;
08500 END ELSE
08600 BEGIN
08700 ARRBLT(COEFF[PTR4],REAL_DATA[1],18);
08800 PTR4←PTR4+17 END;
08900 END;
09000
09100 SIMPLE MESSAGE PROCEDURE SCREW(REAL VELOCITY);
09200 BEGIN INTEGER I;
09300 IF ARM_EXECUTE THEN BEGIN ARM_EXECUTE←FALSE; RETURN END;
09400 STACK[PTR3←PTR3+1]←'22000000 LOR ((I←VELOCITY*1.0) LAND '777777) END;
09500